# Copyright 2009 David Lee Miller
#
# This file is part of POSSM (Python Open Source Scanning Microscopy).
#
# POSSM is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# POSSM is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License         
# along with POSSM.  If not, see <http://www.gnu.org/licenses/>.
#                                                                           
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #

from controller import time,ws,Queue,n

class Approach(object):
        AXIS1_DIRECTION = ws.AXIS1_DIRECTION
        AXIS2_DIRECTION = ws.AXIS2_DIRECTION
        AXIS1_CLOCK = ws.AXIS1_CLOCK
        AXIS2_CLOCK = ws.AXIS2_CLOCK
        AXIS1_HALT = ws.AXIS1_HALT
        AXIS2_HALT = ws.AXIS2_HALT
        PAR_COMMAND = [0,0,0,0,0,0,0,0]
        
        RETRACT = ws.RETRACT
        HOLD = ws.HOLD
        
        FORWARD  = ws.FORWARD
        REVERSE  = ws.REVERSE
        
        CENTER_DIFF0 = ws.CENTER_DIFF0
        CENTER_DIFF1 = ws.CENTER_DIFF1
        CENTER_DIFF2 = ws.CENTER_DIFF2
        CENTER_DIFF3 = ws.CENTER_DIFF3
        CENTER_DIFF4 = ws.CENTER_DIFF4
        CD_STEP0 = ws.CD_STEP0
        CD_STEP1 = ws.CD_STEP1
        CD_STEP2 = ws.CD_STEP2
        CD_STEP3 = ws.CD_STEP3
        CD_STEP4 = ws.CD_STEP4
        
        CENTER_RETRACT = ws.CENTER_RETRACT
        CENTER_SWEEP = ws.CENTER_SWEEP
        RETRACT_DELAY = ws.RETRACT_DELAY
        RETRACT_NREADS = ws.RETRACT_NREADS
        SWEEP_DELAY = ws.SWEEP_DELAY
        SWEEP_NREADS = ws.SWEEP_NREADS
        
        INCHES = ws.INCHES

        bigcount = [0,0]
        lilcount = [0,0]
        slowrate = ws.slowrate
        fastrate = ws.fastrate
        INCH_DELAY = ws.INCH_DELAY
        tunneling = False
        
        Z_MIN_VOLTS = ws.Z_MIN_VOLTS
        Z_MAX_VOLTS = ws.Z_MAX_VOLTS
        Z_MAX_ERR = ws.Z_MAX_ERR
        SWEEP_TIMEOUT = ws.SWEEP_TIMEOUT
        CENTER_TIMEOUT = ws.CENTER_TIMEOUT
        TUNNELING_TIMEOUT = ws.TUNNEL_TIMEOUT
        WITHDRAW_STEPS = ws.WITHDRAW_STEPS
        
        def __init__(self,inq,outq,zpos,pport,dOut,center,cutoff):
                self.pport = pport
                self.zpos = zpos
                self.dOut = dOut
                self.inq = inq
                self.outq = outq
                if not inq: self.inq = Queue.Queue()
                if not outq: self.outq = Queue.Queue()
                self.Z_CENTER_VOLTS = center
                self.Z_MAX_CUTOFF = cutoff
                self.n_lilsteps = 0
                self.n_bigsteps = 0
        
        def microdelay(self,ticks):
                '''
                Software timer to delay "ticks" microseconds. Accuracy to less 
                than +-4 microseconds barring system delays.
                
                Keyword Arguments:
                ticks -- Number of 4 microsecond ticks to delay
                
                '''
                
                start_time = time.time()
                smallest_delay = 0.000004
                total_time = 0
                while total_time < smallest_delay*ticks:
                        total_time = time.time() - start_time
        
        def setPar(self,bits):
                '''
                Sets the selected bits to the specified values on the digital
                output port. If a bit is already set, nothing changes.
                
                Keyword Arguments:
                bits -- [(bit#,value),(bit#,value),...]
                
                '''
                
                if type(bits) != list: raise 'Invalid type for bits'
                for i in range(len(bits)):
                        if 0 <= bits[i][0] <= 7:
                                self.PAR_COMMAND[bits[i][0]] = bits[i][1]
                val = self.list2UInt8(self.PAR_COMMAND)
                return val
        
        def flipPar(self,bit):
                '''
                Sets the selected bits to the specified values on the digital
                output port. If a bit is already set, nothing changes.
                
                Keyword Arguments:
                bit -- the number of the bit to flip
                
                '''
                
                if not 0<=bit<=7: raise 'Invalid number for bit'
                if self.PAR_COMMAND[bit]: self.PAR_COMMAND[bit] = 0
                else: self.PAR_COMMAND[bit] = 1
                val = self.list2UInt8(self.PAR_COMMAND)
                return val
        
        def list2UInt8(self,list8):
                '''
                Takes a list of 0's and 1's and converts it to an 8-bit
                unsigned int.
                
                Keyword Arguments:
                list8 -- a list of length 8 containing 0's and 1's
                
                '''
                
                if type(list8) != list: raise 'Argument not of type list'
                elif len(list8) != 8: raise 'List does not have length 8'
                for j in list8:
                        if j != 0 and j != 1: raise 'Invalid list elements'
                        else: pass
                
                bitcode = 0
                for i in range(8):
                        bitcode += (0x01 << i)*list8[i]
                return bitcode
        
        def lilstep(self,nsteps,rate):
                '''
                Steps the inchworm nsteps at the given rate.
                
                Keyword Arguments:
                nsteps -- number of steps to take
                rate -- stepping rate
                
                '''
                
                if rate > 125000: rate = 125000
                period = (1000000/rate)
                self.n_lilsteps += nsteps
                
                if nsteps < 0: self.pport.outp(self.setPar([(self.AXIS1_DIRECTION,self.REVERSE)]))
                else: self.pport.outp(self.setPar([(self.AXIS1_DIRECTION,self.FORWARD)]))
                
                for i in range(abs(nsteps)):
                        self.pport.outp(self.flipPar(self.AXIS1_CLOCK))
                        self.microdelay(int(round(period/4.)))
        
                
        def bigstep(self,nsteps):
                self.n_bigsteps += nsteps
                self.lilstep(nsteps * self.INCHES,self.fastrate)
        
        def tip_retract(self,volts):
                '''
                Retracts the tip to the specified error voltage.
                
                Keyword Arguments:
                volts -- Error voltage to retract the tip to
                
                '''
                
                self.dOut.setBits([(self.HOLD,1),(self.RETRACT,0)])
                err = self.zpos.readFew(self.RETRACT_NREADS)
                time.sleep(self.RETRACT_DELAY)
                while volts < err:
                        err = self.zpos.readFew(self.RETRACT_NREADS)
                        time.sleep(self.RETRACT_DELAY)
                #self.dOut.setBits([(self.HOLD,0),(self.RETRACT,0)])
                self.dOut.clrBit(self.HOLD)
                
        
        def tip_sweep(self,volts):
                '''
                Sweeps the tip into the specified error voltage
                and checks for tunneling.
                
                Keyword Arguments:
                volts -- the max sweep voltage
                
                '''
                
                errlist = []
                center_cons = 0 #consecutive center matches
                tunn_cons = 0 #consecutive non-centered tunneling matches
                self.dOut.setBits([(self.RETRACT,1),(self.HOLD,1)])
                start_time = time.time()
                err = self.zpos.readFew(self.SWEEP_NREADS)
                errlist.append(err)
                time.sleep(self.SWEEP_DELAY)
                centered = False
                while err < volts:
                        err = self.zpos.readFew(self.SWEEP_NREADS)
                        errlist.append(err)
                        time.sleep(self.SWEEP_DELAY)
                        if time.time() - start_time > self.TUNNELING_TIMEOUT:
                                        self.tunneling = True
                                        centered = False
                                        print "Tunneling Found"
                                        break
                        else:
                                center_cons = 0
                                tunn_cons = 0
                                self.tunneling = False
                                centered = False
                return centered
        
        def tip_center(self,center):
                '''
                Sweeps the tip into the specified error voltage
                and checks for tunneling.
                
                Keyword Arguments:
                volts -- the max sweep voltage
                
                '''
                
                errlist = []
                self.dOut.setBits([(self.RETRACT,1),(self.HOLD,1)])
                start_time = time.time()
                err = self.zpos.readFew(self.SWEEP_NREADS)
                errlist.append(err)
                time.sleep(self.SWEEP_DELAY)
                centered = False
                while err < center:
                        err = self.zpos.readFew(self.SWEEP_NREADS)
                        errlist.append(err)
                        time.sleep(self.SWEEP_DELAY)
                        if err > center:
                                centered = False
                                break
                        elif err < center and time.time()- start_time > self.CENTER_TIMEOUT:
                                centered = True
                        else:
                                centered = False
                print err
                return centered
        
        def tip_sweep_err(self):
                '''
                Sweeps the tip into the specified error voltage
                and checks for tunneling.
                
                Keyword Arguments:
                volts -- the max sweep voltage
                
                '''
                
                errlist = []
                self.dOut.setBits([(self.RETRACT,1),(self.HOLD,1)])
                start_time = time.time()
                err = self.zpos.readFew(self.SWEEP_NREADS)
                errlist.append(err)
                time.sleep(self.SWEEP_DELAY)
                while 1:
                        err = self.zpos.readFew(self.SWEEP_NREADS)
                        errlist.append(err)
                        time.sleep(self.SWEEP_DELAY)
                        if err > self.Z_MAX_ERR:
                                break
                        elif err < self.Z_MAX_ERR and time.time()- start_time > self.SWEEP_TIMEOUT: break
                        else:
                                pass
                return err
        
        def Safe_Approach(self,volts):
                step = 0
                success = True
                self.start_time = time.time()
		print 'check'
                while not self.tunneling:
			print 'check2'
                        self.tip_sweep(volts)
                        self.tip_retract(-volts)
                        if not self.tunneling:
				print 'check3'
                                self.bigstep(1)
                                step += 1
                                self.outq.put('Step Number: %d'%step)
                                time.sleep(self.INCH_DELAY)
                                print "Step Number: ",step
                        if 'abort' in self.inq.queue:
                                self.outq.put('Aborting approach..')
                                success = False
                                break
		print 'check4'
                return success
        
        def Safe_Center(self,center):
                step = 0
                centered = False
                while not centered:
                        err = self.tip_sweep_err()
                        self.tip_retract(self.CENTER_RETRACT)
                        if err-center > self.CENTER_DIFF0: step = self.CD_STEP0
                        elif err-center > self.CENTER_DIFF1: step = self.CD_STEP1
                        elif err-center > self.CENTER_DIFF2: step = self.CD_STEP2
                        elif err-center > self.CENTER_DIFF3: step = self.CD_STEP3
                        elif err-center > self.CENTER_DIFF4: step = self.CD_STEP4
                        else:
                                step = 1
                                centered = True
                        self.lilstep(step,self.slowrate)
                position = self.tip_sweep_err()
                self.outq.put('Tip centered at: %s Volts'%position)
                print 'Tip centered at: ',position,' Volts'
        
        def Manual_Center(self):
                while 1:
                        self.outq.put('Enter the Number of Little Steps (0 to quit):')
                        while 1:
                                result = self.inq.get(block=True)
                                try:
                                        nsteps = int(result)
                                        break
                                except: self.outq.put('  Need Integer Steps:')
                        if not nsteps == 0:
                                self.lilstep(nsteps,self.slowrate)
                                err = self.tip_sweep_err()
                                self.outq.put('Centered at %f Volts'%err)
                                self.tip_retract(-self.Z_MAX_CUTOFF)
                        else: break

        def Pull_Off(self,wait=True):
                '''
                Pulls off of the block after the approach has
                completed.
                
                '''
                self.tip_retract(self.Z_MIN_VOLTS)
                if wait: res = self.WaitForResistors()
                else: res = True
                if res:
                        self.lilstep(-1000,1000)
                        self.bigstep(-self.WITHDRAW_STEPS)
                else: pass
        
        def Report_Time(self):
                '''
                Sends the total approach time to the user.
                
                '''
                total = time.time()-self.start_time
                minutes = str(int(total/60))
                seconds = str(int(total%60))
                if len(seconds) < 2: seconds = '0'+seconds
                if len(seconds) < 2: seconds = '0'+seconds
                final = minutes+':'+seconds
                self.outq.put('total time: %s'%final)
        
        def WaitForResistors(self):
                '''
                Waits for resistors to be switch before backing copper block
                off of the sample.
                
                '''
                self.outq.put('Turn on Resistors. Send go when ready or abort to quit.')
                while 1:
                        result = self.inq.get(block=True)
                        if result == 'go':
                                cont = True
                                break
                        elif result == 'abort':
                                cont = False
                                break
                return cont
        
        def _Approach(self):
                self.outq.put('Approach Started..\n')
                res = self.Safe_Approach(self.Z_MAX_CUTOFF)
                if res:
                        self.outq.put('Tunneling Found at %d bigsteps'%self.n_bigsteps)
                        print 'Tunneling Found at %d bigsteps'%self.n_bigsteps
                        #self.Safe_Center(self.Z_CENTER_VOLTS)
                        err = self.tip_sweep_err()
                        self.outq.put('Initial Approach Completed. Centered at %f Volts'%err)
			self.tip_retract(-self.Z_MAX_CUTOFF)
                        self.Manual_Center()
                        ###now that centering has completed, step away from sample block
                        #self.Pull_Off(wait=True)
                        err = self.tip_sweep_err()
                        print 'Approach Completed. Centered at %f Volts'%err
                        self.outq.put('Approach Completed. Centered at %f Volts'%err)
                        self.Report_Time()
                else:
                        self.outq.put('Approach Halted.')
                        err = self.tip_sweep_err()


